#!/usr/bin/env python
# coding=utf-8
import numpy as np
import time

class Point3D:
    def __init__(self,x=0,y=0,z=0,roll=0,pitch=0,yaw=0):
        self.x=x
        self.y=y
        self.z=z
        self.roll=roll
        self.pitch=pitch
        self.yaw=yaw
def Inv3dof(
        x, y, pitch,
        L1=0.05, L2=0.05, L3=0.022,
        j0ScanRange=[1.4, -1.4, -0.001],\
    j1Range=[-1.57,0.1],\
    j2Range=[-1.4,1.4]):
    '''
        平面层面上从位置到足部姿态的解算
    '''
    rotate_3=pitch+np.pi/2
    #   解算第二关节点的位置
    p2X=L3*np.cos(rotate_3)-x;p2Y=y+L3*np.sin(rotate_3)
    for theta in np.arange(j0ScanRange[0],j0ScanRange[1],j0ScanRange[2]):
        '''
            扫描关节J0
        '''
        p1X=L1*np.sin(theta);p1Y=-L1*np.cos(theta)
        diffX=p2X-p1X;diffY=p1Y-p2Y #   这里P1-P2的原因是因为Y坐标是负的
        l2Predicted=np.sqrt(np.power(diffX,2)+np.power(diffY,2))
        if np.abs(l2Predicted-L2)>0.002:continue
        theta2=np.arctan2(diffX,diffY)-theta
        theta3=pitch-theta2-theta
        if j1Range[0]<theta2<j1Range[1] and j2Range[0]<theta3<j2Range[1]:
            return True,[theta,theta2,theta3]
    return False,[]




if __name__ == '__main__':
    t = time.time()
    ret = Inv3dof(-0, -0.1,0.0)
    print(ret)
    print(time.time() - t)
